
#if defined quaternion_inc
 #endscript
#endif

#include <float>



stock Q_ToEuler(Float:qx, Float:qy, Float:qz, Float:qw, &Float:ex, &Float:ey, &Float:ez)
{
	new Float:sqw = qw*qw;
	new Float:sqx = qx*qx;
	new Float:sqy = qy*qy;
	new Float:sqz = qz*qz;

	ex = atan2(2.0 * (qx*qy + qz*qw), sqx - sqy - sqz + sqw);
	ey = asin(-2.0 * (qx*qz - qy*qw));
	ez = atan2(2.0 * (qy*qz + qx*qw), -sqx - sqy + sqz + sqw);

	return;
}

stock Q_ToEulerEx(Float:qx, Float:qy, Float:qz, Float:qw, &Float:ex, &Float:ey, &Float:ez)
{
	return Q_ToEuler(-qx, -qy, -qz, -qw, ex, ey, ez);
}

stock Q_ToEuler2(Float:qx, Float:qy, Float:qz, Float:qw, &Float:x, &Float:y, &Float:z)
{
	//new Float:qw2 = qw*qw;
	new Float:qx2 = qx*qx;
	new Float:qy2 = qy*qy;
	new Float:qz2 = qz*qz;
	new Float:test= qx*qy + qz*qw;
	if (test > 0.4999999999999) {
		y = -2*atan2(qx,qw);
		z = -90;
		x = 0;
		#if defined IPL_DEBUG
		printf("%f, %f %f %f", test, x, y ,z);
		#endif
		return;
	}
	if (test < -0.49999999999) {
		y = 2*atan2(qx,qw);
		z = 90;
		x = 0;
		#if defined IPL_DEBUG
		printf("%f, %f %f %f", test, x, y ,z);
		#endif
		return;
	}
	new Float:h = atan2(2*qy*qw-2*qx*qz,1-2*qy2-2*qz2);
	new Float:a = asin(2*qx*qy+2*qz*qw);
	new Float:b = atan2(2*qx*qw-2*qy*qz,1-2*qx2-2*qz2);
	y = h;//180/PI;
	z = a;//180/PI;
	x = b;//180/PI;

}

stock Q_ToEuler3(Float:rX,Float:rY,Float:rZ,Float:rW,&Float:RX,&Float:RY,&Float:RZ)
{
	new Float:sqx = rX^2,Float:sqy = rY^2,
		Float:sqz = rZ^2,Float:sqw = rW^2,
		Float:unit = sqx + sqy + sqz + sqw,
		Float:test = rX*rY+rZ*rW;
	if(test > 0.4999999999999 * unit)
	{
		RX = 2 * atan2(rX,rW);
		RZ = PI/2;
		RY = 0;
	}
	else if(test < -0.4999999999999 * unit)
	{
		RX = -2 * atan2(rX,rW);
		RZ = -PI/2;
		RY = 0;
	}
	else
	{
		new Float:tmp = (2*rY*rW)-(2*rX*rZ);
		RX = atan2(tmp,sqx-sqy-sqz+sqw);
		RZ = asin(2 * test/unit);
		tmp = (2*rX*rW)-(2*rY*rZ);
		RY = atan2(tmp,-sqx+sqy-sqy+sqw);
	}
	/*
	RX = float(floatround(RX));
	RY = float(floatround(RY));
	RZ = float(floatround(RZ));
	*/
}


stock Q_ToEuler4(Float:qx, Float:qy, Float:qz, Float:qw, &Float:x, &Float:y, &Float:z)
{
	if(qw > 1) qw = 1.0; // if w>1 acos and sqrt will produce errors, this cant happen if quaternion is normalised
	//new Float:angle = 2 * acos(qw);
	new Float:s = floatsqroot(1-qw*qw); // assuming quaternion normalised then w is less than 1, so term always positive.
	if (s < 0.001)
	{
		// test to avoid divide by zero, s is always positive due to sqrt
		// if s close to zero then direction of axis not important
		x = qx; // if it is important that axis is normalised then replace with x=1; y=z=0;
		y = qy;
		z = qz;
	}
	else
	{
		x = qx / s; // normalise axis
		y = qy / s;
		z = qz / s;
	}
}
